#include <MsTimer2.h>

#define DEBUG 0

#define Ta 50 // Tempo de amostragem (em milisegundos)
#define STEP_TIME 10 // Tempo de cada degrau (em segundos)
#define FRENTE 1
#define TRAS 0
#define PPR_A 330 // Pulses Per Revolution - Motor A
#define PPR_B 430 // Pulses Per Revolution - Motor B


#include "WProgram.h"
void setup();
void Amostragem();
void conta_A();
void conta_B();
void loop();
long int contador_A[2] = {0,0};
long int contador_B[2] = {0,0};
long double tempo = 0;
int index = 1;
float velocidade_A[3];
float velocidade_B[3];

static boolean flag = LOW;

int motor_A_speed = 0; // velocidade inicial 0 <= motor_A_speed <= 250  
int motor_B_speed = 0;
int sentido_rot;
int numero_voltas = 200;

//---------Conexao dos pinos MOTOR_A---------//
int b1_A = 7; //motor A bit1 
int b2_A = 8; //motor A bit1 
int EN_A = 9; // Enable motor A
// Encoder_motor_A -> digital 02

//---------Conexao dos pinos MOTOR_B---------//
int b1_B = 5; //motor B bit1 
int b2_B = 6; //motor B bit1 
int EN_B = 10; // Enable motor B
// Encoder_motor_B -> digital 03

void setup()
{
  Serial.begin(9600);

  //MsTimer2::set(Ta, Amostragem);

  pinMode(EN_A,OUTPUT);
  pinMode(b1_A,OUTPUT);
  pinMode(b2_A,OUTPUT);
  pinMode(EN_B,OUTPUT);
  pinMode(b1_B,OUTPUT);
  pinMode(b2_B,OUTPUT);


  pinMode(13, OUTPUT); // LED pin
  pinMode(2, INPUT); // interrupt 0 - Encoder_Motor_A
  pinMode(3, INPUT); // interrupt 1 - Encoder_Motor_B

  // Default = FRENTE
  digitalWrite(b1_A, HIGH);
  digitalWrite(b2_A, LOW);
  digitalWrite(b1_B, HIGH);
  digitalWrite(b2_B, LOW);
  
  sentido_rot = FRENTE;

  attachInterrupt(0, conta_A, CHANGE); // Encoder_motor_A -> digital 02
//  attachInterrupt(1, conta_B, CHANGE); // Encoder_motor_B -> digital 03
  
  //MsTimer2::start(); 
  
  delay(100);
  
}

void Amostragem()
{
  tempo = tempo + Ta;

  velocidade_A[0] = ( (contador_A[1]-contador_A[0])*(2*PI/PPR_A))/Ta; // velocidade em rad/ms
  velocidade_B[0] = ( (contador_B[1]-contador_B[0])*(2*PI/PPR_B))/Ta; // velocidade em rad/ms
  
  //Serial.println( (int)(velocidade[0]*100000) );

  contador_A[0] = contador_A[1];
  contador_B[0] = contador_B[1];
}


void conta_A()
{

  contador_A[1] = contador_A[1]+1;

  if( DEBUG )
  {
    digitalWrite(13,flag);
    flag = !flag;
  }

}


void conta_B()
{

  contador_B[1] = contador_B[1]+1;

  if( DEBUG )
  {
    digitalWrite(13,flag);
    flag = !flag;
  }

}


void loop()
{

  while( (contador_A[1] < 2*PPR_A) )
  {
       if( motor_A_speed < 200 )
       {
         motor_A_speed+=2;
         motor_B_speed+=2;
         analogWrite( EN_A, motor_A_speed );
         analogWrite( EN_B, motor_B_speed );
         delay(5);
       }
       
       delay(5);
       
  }
  
       motor_A_speed = 0;
       motor_B_speed = 0;
       
       digitalWrite(b1_A, HIGH);
       digitalWrite(b2_A, HIGH);
       digitalWrite(b1_B, HIGH);
       digitalWrite(b2_B, HIGH);        
  
 analogWrite( EN_A, motor_A_speed );
 analogWrite( EN_B, motor_B_speed );
 
  
 if( Serial.available() )
     switch( Serial.read() )
     {
     
     case 'r' : // read counter
   
       Serial.println(contador_A[1]);
       Serial.println(contador_B[1]);
   
     break; 
   
     case 'z' : //zerar
   
       contador_A[1] = 0;
       contador_A[0] = 0;
       contador_B[1] = 0;
       contador_B[0] = 0;
       
     break;
   
     case 'v' : // velocidade
   
       Serial.println( (int)(velocidade_A[0]*100) );
       Serial.println( (int)(velocidade_B[0]*100) );
       
     break; 
   
     case 's' : // Flush Stop motor 
   
       motor_A_speed = 0;
       motor_B_speed = 0;
       
     break;
   
   
     case 't' : // Trava os motores 
   
       digitalWrite(b1_A, HIGH);
       digitalWrite(b2_A, HIGH);
       digitalWrite(b1_B, HIGH);
       digitalWrite(b2_B, HIGH);
   
     break;
   
   
     case 'a' : // aumenta velocidade
   
       if( (motor_A_speed <= 240) &&  (motor_B_speed <= 240) )
       {
          motor_A_speed = motor_A_speed + 10;
          motor_B_speed = motor_B_speed + 10;
          Serial.println( motor_A_speed );
          Serial.println( motor_B_speed );
       }
   
     break;
   
   
     case 'd' : // diminui velocidade
   
       if( (motor_A_speed >= 10) && (motor_B_speed >= 10) )
       {
         motor_A_speed = motor_A_speed - 10;
         motor_B_speed = motor_B_speed - 10;
         Serial.println( motor_A_speed );
         Serial.println( motor_B_speed );
       }
   
     break;
   
     case 'i' : // inverte sentido de rotacao do motor
   
       if( sentido_rot == FRENTE )
       {
         sentido_rot = TRAS;
         digitalWrite(b1_A, LOW);
         digitalWrite(b2_A, HIGH);
         digitalWrite(b1_B, LOW);
         digitalWrite(b2_B, HIGH);
       }
       else
       {
         sentido_rot = FRENTE;
         digitalWrite(b1_A, HIGH);
         digitalWrite(b2_A, LOW);
         digitalWrite(b1_B, HIGH);
         digitalWrite(b2_B, LOW);
       }
   
     break;
     
     case 'y':
     
       for( int i=0; i < 180; i+=2)
       {
         motor_A_speed++;
         motor_B_speed++;
         analogWrite( EN_A, motor_A_speed );
         analogWrite( EN_B, motor_B_speed );
         delay(10);
       }
       
       delay(2000);
       
       motor_A_speed = 0;
       motor_B_speed = 0;
       
       digitalWrite(b1_A, HIGH);
       digitalWrite(b2_A, HIGH);
       digitalWrite(b1_B, HIGH);
       digitalWrite(b2_B, HIGH);        
       
    break;
    
     }

}






int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

